package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Victor;
import java.util.TimerTask;

/* Tasks
 * Move to a given height (neutral, one, two three)
 * Retract to neutral state
 * Self-calibrate
 */
/**
 *
 * @author buttercream
 */
public class Arm extends TimerTask
{
    private static double ADJUST_THRESHHOLD = 0.1;

    private Joystick joyStick;
    private PIDController pid;
    private MagneticEnc encoder;
    private Jaguar motor;
    
    boolean calibrated = false;

    public Arm(Joystick joyStick)
    {
         encoder = new MagneticEnc( Constants.ARM_CHNL);
         motor = new Jaguar(Constants.CRIO_DIGITAL_SIDECAR, Constants.ARM_MOTOR);
         pid = new PIDController( Constants.ARM_PID_P, Constants.ARM_PID_I, 
                 Constants.ARM_PID_D, encoder, motor);
//         pid.setTolerance(Constants.PID_TOLERANCE);
//         pid.setInputRange(-1, 1);
//         pid.setContinuous();
         pid.enable();
    }

    public void run()
    {
        
        //Be sure to wait until calibration is finished before allowing arm
        //to do anything!
        if (!calibrated) {
//            if (!topSwitch.get()) {
//                //lifterMotor.set(TOP);
//                lifterTop();
//                return;
//            } else {
//                calibrated = true;
//            }
        }
        else
        {

        //Check to see if a button is pressed. If it is, figure out what level
        //to move it to. Set PID to go to that height.
        //Remember to consider that a joystick button may be pressed through
        //multiple cycles. Won't necessarily be a single press in a single
        //iteration
            getCommandFromJoystick();
        }
    }

    private void getCommandFromJoystick()
    {
//        if ( joyStick.getRawButton(  BTN_LIFTER_TOP ) )
//        else if(joyStick.getRawButton(BTN_LIFTER_BOTTOM))
        if      ( isButtonPressed( Constants.BTN_FLOOR ))
            floor();
        else if ( isButtonPressed( Constants.BTN_TUCKED ))
            tucked();
        else if ( isButtonPressed( Constants.BTN_TOP_ROW_SIDE ))
            topRowSide();
        else if ( isButtonPressed( Constants.BTN_TOP_ROW_CENTER ))
            topRowCenter();
        else if ( isButtonPressed( Constants.BTN_MIDDLE_ROW_SIDE ))
            middleRowSide();
        else if ( isButtonPressed( Constants.BTN_MIDDLE_ROW_CENTER ))
            middleRowCenter();
        else if ( isButtonPressed( Constants.BTN_BOTTOM_ROW_SIDE ))
            bottomRowSide();
        else if ( isButtonPressed( Constants.BTN_BOTTOM_ROW_CENTER ))
            bottomRowCenter();

        else manualAdjust();
    }

    private boolean isButtonPressed( int button )
    {
        return joyStick.getRawButton( button );
    }

    private void goToPosition( double position )
    {
        pid.setSetpoint(position);
    }

    public void floor()
    {
        goToPosition( Constants.ARM_FLOOR );
    }

    public void tucked()
    {
        goToPosition( Constants.ARM_TUCKED );
    }

    public void topRowSide()
    {
        goToPosition( Constants.ARM_TOP_ROW_SIDE );
    }

    public void topRowCenter()
    {
        goToPosition( Constants.ARM_TOP_ROW_CENTER );
    }

    public void middleRowSide()
    {
        goToPosition( Constants.ARM_MIDDLE_ROW_SIDE );
    }

    public void middleRowCenter()
    {
        goToPosition( Constants.ARM_MIDDLE_ROW_CENTER );
    }

    public void bottomRowSide()
    {
        goToPosition( Constants.ARM_BOTTOM_ROW_SIDE);
    }

    public void bottomRowCenter()
    {
        goToPosition( Constants.ARM_BOTTOM_ROW_CENTER );
    }

    private void manualAdjust()
    {
        // read joystick position to see if increnmental adjustments is requested
        double xVal = joyStick.getX();
        if ( Math.abs(xVal) > ADJUST_THRESHHOLD )
        {
            double position = pid.get();
            double adjustAmt = (xVal>0) ? 1.0 : -1.0;
            pid.setSetpoint(position + adjustAmt);
        }
    }
}
